#include "Inf_PCA9557.h"
#include "driver/gpio.h"
#include "driver/i2c.h"
#include "esp_log.h"

// 6D姿态传感器 芯片地址
#define PCA9557_SENSOR_ADDR 0x19
// 超时时间 1000ms = 1S
#define I2C_MASTER_TIMEOUT_MS 1000
#define CONFIG_FREERTOS_HZ 100
// portTICK_PERIOD_MS 1ms 多少HZ
#define Inf_portTICK_PERIOD_MS ((TickType_t)1000 / CONFIG_FREERTOS_HZ)

/**
 * @brief 读取
 *
 */
esp_err_t Inf_PCA9557_Read(uint8_t reg_addr, uint8_t *data, size_t len) {
  return i2c_master_write_read_device(I2C_NUM_0, PCA9557_SENSOR_ADDR, &reg_addr, 1, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

/**
 * @brief 发送
 *
 */
esp_err_t Inf_PCA9557_Send(uint8_t reg_addr, uint8_t data) {
  uint8_t write_buf[2] = {reg_addr, data};
  return i2c_master_write_to_device(I2C_NUM_0, PCA9557_SENSOR_ADDR, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
}

/**
 * @brief
 *
 */
void Inf_PCA9557_Init(void) {
  // 初始化,寄存器数据
  Inf_PCA9557_Send(PCA9557_OUTPUT_PORT, 0x05);
  // 修改寄存器地址
  Inf_PCA9557_Send(PCA9557_CONFIGURATION_PORT, 0xf8);
}

// 控制某个GPIO引脚,输出高低电平
esp_err_t Inf_QCA9557_Gpio_Control(uint8_t gpio_bit, uint8_t value) {
  uint8_t data;
  esp_err_t res = ESP_FAIL;
  Inf_PCA9557_Read(PCA9557_OUTPUT_PORT, &data, 1);
  res = Inf_PCA9557_Send(PCA9557_OUTPUT_PORT, SET_BITS(data, gpio_bit, value));

  return res;
}

// 控制 PCA9557_LCD_CS 引脚输出高低电平 参数0输出低电平 参数1输出高电平
void Inf_lcd_cs(uint8_t level) { Inf_QCA9557_Gpio_Control(LCD_CS_GPIO, level); }

// 控制 PCA9557_PA_EN 引脚输出高低电平 参数0输出低电平 参数1输出高电平
void Inf_pa_en(uint8_t level) { Inf_QCA9557_Gpio_Control(PA_EN_GPIO, level); }

// 控制 PCA9557_DVP_PWDN 引脚输出高低电平 参数0输出低电平 参数1输出高电平
void Inf_dvp_pwdn(uint8_t level) { Inf_QCA9557_Gpio_Control(DVP_PWDN_GPIO, level); }